#ifndef ARMMOVE_HPP
#define ARMMOVE_HPP

#include <thread>
#include <mutex>

#include "../urct/urcontrolcamera.hpp"



static void ArmMove(int x, int y, std::mutex &mtx)
{
    if(mtx.try_lock())
    {
        std::cout<<"go to pixel ["<<x<<","<<y<<"]\n"<<std::flush;
        URControlCamera urcc;
        char *ipUR="192.168.1.13";
        char *ipPC="192.168.1.14";


        urcc.PointToPlane(cv::Vec2d(x,y), cv::Vec6d(0,0,0.25,0,0,CV_PI/4), ipUR, ipPC, 0.1, 0.2);

        urcc.PointToPlane(cv::Vec2d(x,y), cv::Vec6d(0,0,0.22,0,0,CV_PI/4), ipUR, ipPC, 0.1, 0.2);

        cv::Vec6d urp=cv::Vec6d(0.163924,-0.312702,0.384971,2.78624,1.44594,0.00952911);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5]);



        mtx.unlock();
    }
}


static void ArmMove4(std::mutex &mtx)
{
    if(mtx.try_lock())
    {
        //        std::cout<<"go to pixel ["<<x<<","<<y<<"]\n"<<std::flush;
        URControlCamera urcc;
        //        char *ipUR="192.168.1.13";
        //        char *ipPC="192.168.1.14";


        char ipUR[]="192.168.0.200";
        char ipPC[]="192.168.0.102";


        //        urcc.PointToPlane(cv::Vec2d(x,y), cv::Vec6d(0,0,0.25,0,0,CV_PI/4), ipUR, ipPC, 0.1, 0.2);

        //        urcc.PointToPlane(cv::Vec2d(x,y), cv::Vec6d(0,0,0.22,0,0,CV_PI/4), ipUR, ipPC, 0.1, 0.2);

        //        cv::Vec6d urp=cv::Vec6d(0.163924,-0.312702,0.384971,2.78624,1.44594,0.00952911);

        //        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5]);

        float mm=0;


        URControl::RG2(ipUR, ipPC, mm);

        mm=80;

        URControl::RG2(ipUR, ipPC, mm);

        mtx.unlock();
    }
}

static void ArmMove2(cv::Vec2d cornerPixel[4], std::mutex &mtx)
{
    if(mtx.try_lock())
    {
        ToST tst;

        tst.StartUpdate();

        URControlCamera urcc;
        //        char *ipUR="192.168.1.13";
        //        char *ipPC="192.168.1.14";

        char ipUR[]="192.168.0.200";
        char ipPC[]="192.168.0.102";

        double openmm;

        double a=0.2;
        double v=0.4;

        urcc.PickRectangle(openmm, cornerPixel,cv::Vec6d(0,0,0.21,0,0,CV_PI/4),ipUR,ipPC,a,v);

#if 1

        std::cout<<"send go\n"<<std::flush;
        unsigned char num='5';
        tst.SendMsgUChar(&num,1);
        while(!tst.IsAGVStop()){ std::this_thread::yield(); }
        //        tst.EndUpdate();
#endif


        cv::Vec6d urp;
        urp=cv::Vec6d(-0.511212,-0.0794407,0.331097,0.763605,-2.102,-0.654246);

        urp=cv::Vec6d(-0.518745,-0.0684919,-0.0381951,1.26058,-2.86865,-0.0722171);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);

        URControl::RG2(ipUR, ipPC, openmm);

        urp=cv::Vec6d(-0.151424,-0.270796,0.43873,-1.30697,2.84104,-0.01504);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);

        URControl::RG2(ipUR, ipPC, 0);
#if 0
        while(k!='g')
        {
            std::this_thread::yield();
        }
        k=0;
#else
        std::this_thread::sleep_for(std::chrono::seconds(1));
#endif

#if 1
        num='8';
        tst.SendMsgUChar(&num,1);
        while(!tst.IsAGVStop2()){ std::this_thread::yield(); }
#endif


#if 0
        while(k!='g')
        {
            std::this_thread::yield();
        }
        k=0;
#else
        std::this_thread::sleep_for(std::chrono::seconds(1));
#endif

#if 1
        num='2';
        tst.SendMsgUChar(&num,1);
        while(!tst.IsAGVStop3()){ std::this_thread::yield(); }
#endif


        tst.EndUpdate();

        std::this_thread::sleep_for(std::chrono::seconds(4));

        mtx.unlock();
#if 0
        usleep(1e7);

        brun=false;
#endif


        std::cout<<"end move\n"<<std::flush;



    }

    delete []cornerPixel;

    std::cout<<"end thread\n"<<std::flush;
}


static void ArmMove3(cv::Vec2d cornerPixel[4], std::mutex &mtx)
{
    if(mtx.try_lock())
    {
        URControlCamera urcc;
        char *ipUR="192.168.1.13";
        char *ipPC="192.168.1.14";

        double openmm;

        double a=0.2;
        double v=0.4;

        urcc.PickRectangle(openmm, cornerPixel,cv::Vec6d(0,0,0.21,0,0,CV_PI/4),ipUR,ipPC,a,v);



        cv::Vec6d urp;



        URControl::RG2(ipUR, ipPC, openmm);

        urp=cv::Vec6d(-0.151424,-0.270796,0.43873,-1.30697,2.84104,-0.01504);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);

        URControl::RG2(ipUR, ipPC, 0);

        mtx.unlock();
    }

    delete []cornerPixel;
}


#endif
